/*
    Project:        Home Power Control
    File:           HPCBarometer.cpp
    Author:         christian@karunamusic.at
                    Code based on this: http://cognitivedissonance.ca/cogware/BMP085/BMP085_C++.tar.bz2
    Date:           02.2015
    Description:    Sensor (BMP180) to read the barometric pressure.
 */

#include <QString>
#include <QThread>
#include <wiringPiI2C.h>
#include <unistd.h>
#include <Logger/HPCLogger.h>
#include "HPCValue.h"
#include "HPCBarometer.h"

#include <iostream>

const double HPCBarometer::SEA_LEVEL_PRESSURE = 101.325;
const int HPCBarometer::CONTROL_REG = 0xf4;
const int HPCBarometer::DATA_REG = 0xf6;
const int HPCBarometer::TEMP_COMMAND = 0x2e;
const double HPCBarometer::ALT_EXP = 1.0 / 5.255;

HPCBarometer::HPCBarometer(const QString& name, quint32 id, quint8 i2caddr )
    : HPCSensor( TSensorType::HPCBarometer,name,id,i2caddr ), ok(true), err(""), oss(OSS_ULTRAHIGH), hiRes(true)
{
    m_I2C = wiringPiI2CSetup( i2caddr );
    ac1 = readWord(0xaa);
    ac2 = readWord(0xac);
    ac3 = readWord(0xae);
    ac4 = (uint16_t)readWord(0xb0);
    ac5 = (uint16_t)readWord(0xb2);
    ac6 = (uint16_t)readWord(0xb4);
    b1 = readWord(0xb6);
    b2 = readWord(0xb8);
    mb = readWord(0xba);
    mc = readWord(0xbc);
    md = readWord(0xbe);
}

HPCBarometer::~HPCBarometer()
{
    close(m_I2C);
}

QString HPCBarometer::asString() const
{
    QString ret = HPCSensor::asString();

    ret += " (i2c address)";
    return ret;
}

void HPCBarometer::Measure()
{
    TReading r = getBoth();
    HPCValue res;
    res.V() = adjustedPressure(r);
    LOG( trace, Name() << ": measured value: " << res.V().toFloat() << " (temp: " << getCelcius() << ")" );
    m_Value = res;
}

float HPCBarometer::getCelcius()
{
    return getCelcius( getB5Value() );
}

HPCBarometer::TReading HPCBarometer::getBoth()
{
    // Get uncompensated pressure.
    int ucPress;
    if ( wiringPiI2CWriteReg8(m_I2C, CONTROL_REG, (oss << 6) + 0x34) != 0 )
    {
        THROW("cannot write to control register" );
    }

    unsigned int duration = oss * 5 + 5;
    if ( oss == 3 )
    {
        duration += 5;
    }
    QThread::msleep(duration);

    int msb = wiringPiI2CReadReg8(m_I2C, DATA_REG);
    int lsb = wiringPiI2CReadReg8(m_I2C, DATA_REG + 1);
    int xlsb = 0;
    if ( hiRes )
    {
        xlsb = wiringPiI2CReadReg8(m_I2C, DATA_REG + 2);
    }
    if ( msb == -1 || lsb == -1 || xlsb == -1 )
    {
        THROW("cannot read data register");
    }

    ucPress = ((msb << 16) + (lsb << 8) + xlsb) >> (8 - oss);
    // Calculate true pressure.
    long b5 = getB5Value();
    long b6 = b5 - 4000;
    long x1 = (b2 * (b6 * b6 >> 12)) >> 11;
    long x2 = ac2 * b6 >> 11;
    long x3 = x1 + x2;
    long b3 = (((ac1 * 4 + x3) << oss) + 2) >> 2;
    x1 = ac3 * b6 >> 13;
    x2 = (b1 * (b6 * b6 >> 12)) >> 16;
    x3 = ((x1 + x2) + 2) >> 2;
    unsigned long b4 = ac4 * (unsigned long)(x3 + 32768) >> 15;
    unsigned long b7 = ((unsigned long)ucPress - b3) * (50000 >> oss);
    long p;
    p = ( b7 < 0x80000000 ) ? (b7 * 2) / b4 : (b7/b4) * 2;
    long p8 = p >> 8;
    x1 = p8 * p8;
    x1 *= 3038;
    x1 >>= 16;
    x2 = (-7357 * p) >> 16;
    p += (x1 + x2 + 3791) >> 4;

    // Return struct with temperature.
    HPCBarometer::TReading data = {
        getCelcius(b5),
        (float)p / 1000.0f
    };
    return data;
}

float HPCBarometer::adjustedPressure( HPCBarometer::TReading raw )
{
    // Found formula using non-metric units...
    float tf = celcius2Fahrenheit(raw.celcius);
    float inches = kPa2inHg(raw.kPa);
    float adj = inches * ((tf - 28.63) / (1.1123f * tf + 10978.0f));
    return 3.38638816f * adj + raw.kPa;
}

float HPCBarometer::celcius2Fahrenheit( float t )
{
    return 1.8f * t + 32.0f;
}

float HPCBarometer::kPa2inHg( float t )
{
    return t / 3.38638816f;
}

float HPCBarometer::getCelcius( long b5 )
{
    long t = (b5 + 8) >> 4;
    return (float)t / 10.0f;
}

long HPCBarometer::getB5Value()
{
    if ( wiringPiI2CWriteReg8( m_I2C, CONTROL_REG, TEMP_COMMAND ) != 0 )
    {
        THROW("cannot write to control register");
    }
    QThread::msleep(5);
    long x1 = (readWord(DATA_REG) - ac6) * ac5 >> 15;
    long x2 = (mc << 11) / (x1 + md);
    return x1 + x2;
}

qint16 HPCBarometer::readWord( int addr )
{
    int word = wiringPiI2CReadReg16( m_I2C, addr );
    if (word == -1)
    {
        THROW("cannot read from register " << addr);
    }
    // The sensor is big-endian!
    return (int16_t)((word >> 8) + ((word & 0xff) << 8));
}
